/*
 * This file is part of the Sx Framework Library.
 * 
 * Copyright (C) 2013 University of Colorado Denver
 * <min.choi@ucdenver.edu> <shane.transue@ucdenver.edu>
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy 
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 
 * copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in 
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
 * DEALINGS IN THE SOFTWARE.
 */
#include <sxSelfCCD_MSM_CollisionDetectionHandler.h>
#include <sxVertexTriangleCollisionEvent.h>
#include <sxEdgeEdgeCollisionEvent.h>

Sx::SelfCCD::Handlers::Detection::SelfCCD_MSM_CollisionDetectionHandler::SelfCCD_MSM_CollisionDetectionHandler() {
	this->model = nullptr;
	this->collisionData = nullptr;
}

Sx::SelfCCD::Handlers::Detection::SelfCCD_MSM_CollisionDetectionHandler::~SelfCCD_MSM_CollisionDetectionHandler() {
	delete this->model;
	/* Do not delete this->collisionData */
}

/* This function extracts the vertex information from the provided set of
 * mass-spring meshes and stores them in a Self-CCD vector list.
 */
bool ConstructCCDVertexArray(Sx::Util::ArrayList<std::shared_ptr<MassSpringMesh> >& objects, vec3f_list& vertices, Sx::Util::ArrayList<Sx::SelfCCD::Handlers::Detection::CCD_CollisionLookupPair>& collisionLookupTable) {
	if ( objects.size() == 0 ) {
		std::cerr << "[SelfCCD_MSM_CollisionDetectionHandler:ConstructCCDVertexArray] Error: Array contains no objects." << std::endl;
		std::cerr << "  SelfCCD vertex array not constructed." << std::endl;
		return false;
	}

	//--------------------------------------------------------------------------
	// Determine the total number of vertices to pre-allocate the array.
	//--------------------------------------------------------------------------
	unsigned int totalArraySize = 0;
	for ( unsigned int i = 0; i < objects.size(); i++ ) {
		std::shared_ptr<Sx::Simulation::Objects::MassSpringMesh>& curMesh = objects[i];

		if ( curMesh == nullptr ) {
			std::cerr << "[SelfCCD_MSM_CollisionDetectionHandler:ConstructCCDVertexArray] Error: Nullptr mesh encountered." << std::endl;
			std::cerr << "  Aborting Self-CCD vertex array construction." << std::endl;
			return false;
		}

		totalArraySize += curMesh->getNumNodes();
	}

	//--------------------------------------------------------------------------
	// Iterate through each object and add the vertices to the self-ccd list.
	//--------------------------------------------------------------------------
	if ( vertices.size() != totalArraySize )
		vertices.resize(totalArraySize);

	if ( collisionLookupTable.size() != totalArraySize )
		collisionLookupTable.resize(totalArraySize);

	unsigned int globalVertexIndex = 0;
	Eigen::Vector3d position, tPosition;
	for ( unsigned int i = 0; i < objects.size(); i++ ) {
		std::shared_ptr<Sx::Simulation::Objects::MassSpringMesh>& curMesh = objects[i];

		for ( unsigned int x = 0; x < curMesh->getNumNodes(); x++ ) {
			position = curMesh->getNodePosition(x);
			// UPDATE: Transformation application
			tPosition = Sx::Math::ApplyTransform(curMesh->getObject()->getTransformation().toMatrix4f(), position);
			vertices[globalVertexIndex] = vec3f(static_cast<float>(tPosition.x()), static_cast<float>(tPosition.y()), static_cast<float>(tPosition.z()));

			collisionLookupTable[globalVertexIndex] = Sx::SelfCCD::Handlers::Detection::CCD_CollisionLookupPair(x, curMesh);
			globalVertexIndex++;
		}
	}

	return true;
}

/* This function extracts the triangle information from the provided list of
 * mass-spring meshes and stores it into a indexed-triangle list.
 */
bool ConstructCCDTriangleArray(Sx::Util::ArrayList<std::shared_ptr<MassSpringMesh> >& objects, tri_list& triangles) {
	if ( objects.size() == 0 ) {
		std::cerr << "[SelfCCD_MSM_CollisionDetectionHandler:ConstructCCDTriangleArray] Error: Array contains no objects." << std::endl;
		std::cerr << "  SelfCCD triangle array not constructed." << std::endl;
		return false;
	}

	//--------------------------------------------------------------------------
	// Determine the total number of faces to pre-allocate the array.
	//--------------------------------------------------------------------------
	unsigned int totalArraySize = 0;
	for ( unsigned int i = 0; i < objects.size(); i++ ) {
		std::shared_ptr<Sx::Simulation::Objects::MassSpringMesh>& curMesh = objects[i];

		if ( curMesh == nullptr ) {
			std::cerr << "[SelfCCD_MSM_CollisionDetectionHandler:ConstructCCDTriangleArray] Error: Nullptr mesh encountered." << std::endl;
			std::cerr << "  Aborting Self-CCD vertex array construction." << std::endl;
			return false;
		}

		totalArraySize += curMesh->getNumFaces();
	}

	//--------------------------------------------------------------------------
	// Iterate through each object and add each face to the list. This
	// conversion also has to make sure that the indices of the triangles are
	// updated to point to the global positions.
	//--------------------------------------------------------------------------
	triangles.resize(totalArraySize);
	unsigned int index = 0;
	unsigned int curTriangleOffset = 0;
	for ( unsigned int i = 0; i < objects.size(); i++ ) {
		std::shared_ptr<Sx::Simulation::Objects::MassSpringMesh>& curMesh = objects[i];

		for ( unsigned int x = 0; x < curMesh->getNumFaces(); x++ ) {
			Sx::Graphics::Primitives::IndiceFace& curFace = curMesh->getFace(x);
			if ( curFace.indices.size() != 3 ) {
				std::cerr << "[SelfCCD_MSM_CollisionDetectionHandler:ConstructCCDTriangleArray] Error: Self-CCD only supports triangulated mesh faces." << std::endl;
				return false;
			}

			triangles[index] = tri3f(curFace.indices[0] + curTriangleOffset, curFace.indices[1] + curTriangleOffset, curFace.indices[2] + curTriangleOffset);
			index++;
		}

		curTriangleOffset += curMesh->getNumNodes();
	}

	return true;
}

bool Sx::SelfCCD::Handlers::Detection::SelfCCD_MSM_CollisionDetectionHandler::construct(Util::ArrayList<std::shared_ptr<MassSpringMesh> >& objects) {
	if ( this->model != nullptr ) {
		delete this->model;
		this->model = nullptr;
	}

	//--------------------------------------------------------------------------
	// Convert the vertices and triangles into the Self-CCD data types.
	//--------------------------------------------------------------------------
	ConstructCCDVertexArray(objects, this->modelVertices, this->collisionLookupTable);

	tri_list modelTriangles;
	ConstructCCDTriangleArray(objects, modelTriangles);
	
	//--------------------------------------------------------------------------
	// Initialize the Self-CCD Model.
	//--------------------------------------------------------------------------
	this->model = new DeformModel(modelVertices, modelTriangles);
	this->model->addCollisionEventListener(this);
	this->model->BuildBVH(true);

	return true;
}

bool Sx::SelfCCD::Handlers::Detection::SelfCCD_MSM_CollisionDetectionHandler::update(Util::ArrayList<std::shared_ptr<MassSpringMesh> >& objects, Physics::Collision::Aggregates::TriangleSoupCollisionEventAggregate<Simulation::Objects::MassSpringMesh>* const collisionData) {
	//--------------------------------------------------------------------------
	// Update the vertices of the Self-CCD collision model.
	//--------------------------------------------------------------------------
	ConstructCCDVertexArray(objects, this->modelVertices, this->collisionLookupTable);

	//--------------------------------------------------------------------------
	// Update the new collision model using the newly collected vertex positions
	//--------------------------------------------------------------------------
	this->model->UpdateVert(this->modelVertices);
	this->model->UpdateBoxes();
	
	//--------------------------------------------------------------------------
	// Assign the internal collision data pointer so that the Self-CCD
	// listener interface functions can create collision events.
	//--------------------------------------------------------------------------
	this->collisionData = collisionData;
	return true;
}

bool Sx::SelfCCD::Handlers::Detection::SelfCCD_MSM_CollisionDetectionHandler::performDetection(Util::ArrayList<std::shared_ptr<MassSpringMesh> >& objects, Physics::Collision::Aggregates::TriangleSoupCollisionEventAggregate<Simulation::Objects::MassSpringMesh>* const collisionData) {

	//--------------------------------------------------------------------------
	// Assign the internal collision data pointer so that the Self-CCD
	// listener interface functions can create collision events.
	//--------------------------------------------------------------------------
	this->collisionData = collisionData;

	//--------------------------------------------------------------------------
	// Modify the bounding volume hierarchy. (Refit = less accurate but faster,
	// Rebuild = slower but more accurate).
	//--------------------------------------------------------------------------
	bool refitBVH = true;
	if ( refitBVH == true ) this->model->RefitBVH(true);
	else this->model->RebuildBVH(true);

	//--------------------------------------------------------------------------
	// Perform the collision detection.
	//--------------------------------------------------------------------------
	this->model->SelfCollide(true);
	return true;
}

void Sx::SelfCCD::Handlers::Detection::SelfCCD_MSM_CollisionDetectionHandler::reset(Util::ArrayList<std::shared_ptr<MassSpringMesh> >& objects) {
	if ( this->model == nullptr ) return;
	ConstructCCDVertexArray(objects, this->modelVertices, this->collisionLookupTable);
	this->model->ResetCounter();
	this->model->Reset(this->modelVertices);
	//this->model->SelfCollide(true);
}

bool Sx::SelfCCD::Handlers::Detection::SelfCCD_MSM_CollisionDetectionHandler::onVertexFaceCollision(unsigned int vertex_index, unsigned int triangle_vertex0_index, unsigned int triangle_vertex1_index, unsigned int triangle_vertex2_index, float t) {
	if ( this->collisionData == nullptr ) return false;

	//--------------------------------------------------------------------------
	// Collision occurred between the objects: object and other.
	// If object == other then there was a self collision between two
	// components in the same mass-spring mesh.
	//--------------------------------------------------------------------------
	std::shared_ptr<Simulation::Objects::MassSpringMesh> object;
	std::shared_ptr<Simulation::Objects::MassSpringMesh> other;

	unsigned int objectVertexIndex;
	unsigned int otherTriangleIndex0;
	unsigned int otherTriangleIndex1;
	unsigned int otherTriangleIndex2;
	
	//--------------------------------------------------------------------------
	// Get vertex and mesh data for object.
	//--------------------------------------------------------------------------
	this->getMesh(vertex_index, object);
	this->getLocalIndex(vertex_index, objectVertexIndex);

	//--------------------------------------------------------------------------
	// Get triangle and mesh data for other object. To get the mesh any of the
	// triangle indices should be interchangeable.
	//--------------------------------------------------------------------------
	this->getMesh(triangle_vertex0_index, other);
	this->getLocalIndex(triangle_vertex0_index, otherTriangleIndex0);
	this->getLocalIndex(triangle_vertex1_index, otherTriangleIndex1);
	this->getLocalIndex(triangle_vertex2_index, otherTriangleIndex2);

	//--------------------------------------------------------------------------
	// Construct a new Vertex-Triangle event with the decoded information.
	//--------------------------------------------------------------------------
	unsigned int eIndex = this->collisionData->eventpool_VertexFace.getFreeEventIndex();
	this->collisionData->eventpool_VertexFace[eIndex] = Physics::Collision::Event::Events::VertexTriangleCollisionEvent<Simulation::Objects::MassSpringMesh>(object, other, static_cast<double>(t), objectVertexIndex, otherTriangleIndex0, otherTriangleIndex1, otherTriangleIndex2);

	return true;
}

bool Sx::SelfCCD::Handlers::Detection::SelfCCD_MSM_CollisionDetectionHandler::onEdgeEdgeCollision(unsigned int edge0_vertex0_index, unsigned int edge0_vertex1_index, unsigned int edge1_vertex0_index, unsigned int edge1_vertex1_index, float t) {
	if ( this->collisionData == nullptr ) return false;
		
	//--------------------------------------------------------------------------
	// Collision occurred between the objects: object and other.
	// If object == other then there was a self collision between two
	// components in the same mass-spring mesh.
	//--------------------------------------------------------------------------
	std::shared_ptr<MassSpringMesh> object;
	std::shared_ptr<MassSpringMesh> other;

	unsigned int objectEdgeIndex0;
	unsigned int objectEdgeIndex1;

	unsigned int otherEdgeIndex0;
	unsigned int otherEdgeIndex1;

	//--------------------------------------------------------------------------
	// Get edge and mesh data for object.
	//--------------------------------------------------------------------------
	this->getMesh(edge0_vertex0_index, object);
	this->getLocalIndex(edge0_vertex0_index, objectEdgeIndex0);
	this->getLocalIndex(edge0_vertex1_index, objectEdgeIndex1);

	//--------------------------------------------------------------------------
	// Get edge and mesh data for other.
	//--------------------------------------------------------------------------
	this->getMesh(edge1_vertex0_index, other);
	this->getLocalIndex(edge1_vertex0_index, otherEdgeIndex0);
	this->getLocalIndex(edge1_vertex1_index, otherEdgeIndex1);

	//--------------------------------------------------------------------------
	// Construct a new Vertex-Triangle event with the decoded information.
	//--------------------------------------------------------------------------
	unsigned int eIndex = this->collisionData->eventpool_EdgeEdge.getFreeEventIndex();
	this->collisionData->eventpool_EdgeEdge[eIndex] = Physics::Collision::Event::Events::EdgeEdgeCollisionEvent<Simulation::Objects::MassSpringMesh>(object, other, static_cast<double>(t), objectEdgeIndex0, objectEdgeIndex1, otherEdgeIndex0, otherEdgeIndex1);

	return true;
}

bool Sx::SelfCCD::Handlers::Detection::SelfCCD_MSM_CollisionDetectionHandler::getMesh(unsigned int globalIndex, std::shared_ptr<MassSpringMesh>& outMesh) {
	if ( globalIndex > this->collisionLookupTable.size() ) return false;
	outMesh = this->collisionLookupTable[globalIndex].mesh;
	return true;
}

bool Sx::SelfCCD::Handlers::Detection::SelfCCD_MSM_CollisionDetectionHandler::getLocalIndex(unsigned int globalIndex, unsigned int& outLocalIndex) {
	if ( globalIndex > this->collisionLookupTable.size() ) return false;
	outLocalIndex = this->collisionLookupTable[globalIndex].localIndex;
	return true;
}
